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ABSTRACT 


A Fortran program was developed to implement a Kalman Filter and Fixed Interval 
Smoothing Algorithm to optimally smooth data tracks generated by the short base-line 
tracking ranges at the Naval Torpedo Station, Keyport, Washington. The program is 
designed to run on a personal computer and requires as input a data file consisting of 
X, Y, and Z position coordinates in sequential order. Data files containing the filtered 
and smoothed estimates are generated Бу the program. This algorithm uses a second 
order linear model to predict a typical target's dynamics. The program listings are in- 
cluded as appendices. 

Several runs of the program were performed using actual range data as inputs. Re- 
sults indicate that the program effectively reduces random noise, thus providing very 
smooth target tracks which closely follow the raw data. Tracks containing data gener- 
ated in an overlap region where one array hands off the target to the next array are 
highlighted. The effects of varving the magnitude of the excitation matrix Q(k) are also 
explored. 

This program is seen as a valuable post-data analysis tool for the current tracking 
range data. In addition, it can easily be modified to provide improved real time, on line 


tracking using the Kalman Filter portion of the algorithm alone. 
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I. INTRODUCTION 


The short base-line array tracking ranges located near the Naval Torpedo Station 
at Keyport, Washington, are used to conduct torpedo testing in support of torpedo de- 
velopment projects. The tracking svstem in use on these ranges consists of a series of 
short base-line arrays which can independently track several subsurface targets by re- 
laving received signals from each target to a tracking computer. The computer processes 
the received array data and calculates target tracks in X,Y,and Z coordinates for display. 
The arrays are distributed over the range so that full coverage of the range 1s achieved, 
and as a result there are several regions on the range where the arravs overlap in their 
coverage. Figure | оп page 2 i1s an illustration of this arrangement [Ref. 1: p. 199]. 
Handoff 1s a term used to describe when the tracking information on a target supplied 
to the tracking computer is shifted from one array to a neighboring array. 

Each short base-line array consists of four omnidirectional hydrophones spaced 50 
feet apart on orthogonal booms as shown in Figure 2 on page 3. The four hydrophones 
are all connected to a common cable which feeds the received signal from each 
hydrophone to the tracking computer. The computer extracts the different times of ar- 


rival of the received signals and calculates X,Y, and Z coordinates based on the following 











equations: 
os о їйї 
Dp e СИ ҮЕ? 
s... 
үн није) (1) 
7 r (1.3) 
2р 
меде: 
V = speed of sound In water, 


D = hydrophone separation distance, 


Tc = tracking signal travel time from target to hydrophone C, 
Tx = tracking signal travel time from target to hydrophone X, 
Ту = tracking signal travel time from target to hydrophone Y, and 





Figure 1. Short Base-lme Array Range Configuration - Fig 2 From Ref 1. 


Tz = tracking signal travel time from target to hydrophone Z. 


A more detailed description of the array and derivation of the equations above can be 
found in [Ref. 2]. 

The tracking signal used on these ranges is a 150 watt phase shift keying (PSK) pulse 
consisting of 47 bits (19 identification bits and 27 telemetry bits) and lasting approxi- 
mately five milliseconds. The O's and I's making up the bit stream are 180 degrees out 
of phase. Correlation processing techniques are used to validate the received signal, thus 
improving the signal to norse ratio. [Ref. 3: p. 9] 

Figures 3 and 4 show examples of a typical track generated bv this system. 
Figure 3 on page d is the track in X vs. Y coordinates generated from data received by 
a single array while Figure 4 on page 5 shows the same tracks in X vs. Z coordinates. 
It is obvious that these tracks are affected by random noise. Figures 5 and 6 show ex- 
amples of a track generated with data from two arrays as would occur in an overlap re- 
gion. Again, the X vs. Y coordinates and X vs. Z coordinates format 1s used. It can 
be seen that the data from the different arrays do not agree. This indicates that bias 
errors are also present in the current tracking system. Tracking accuracy has been im- 


proved by use of the PSK pulse because its sophistication increases the received signal 





Short Base-line Array Configuration. 


Figure 2. 
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Figure 3. Typical Track Generated By A Single Array (X vs Y). 


to noise ratio, but it can be seen from the tracks that a great deal of noise remains. The 
question that now arises 1s how these target tracks can be improved. 

While several approaches to improving the tracking accuracy of the short base-line 
array ranges are possible, the effort here has been on applying a Kalman Filter and 
Fixed Interval Smoothing Algorithm as a post data analysis tool. Quite a bit of work 
has already been done in the area of applying Kalman Filters to underwater tracking 
over the vears [Refs. 4, 5, 6, 7]. More recent efforts have centered around applying an 
optimal smoothing algorithm to the underwater tracking problem [Refs. 8, 9]. Most of 
this work has involved attempts to filter or smooth the transit times prior to their use 
in the tracking equations using an Extended Kalman Filter. In the program developed 


in this research. a Kalman Filter utilizing a second order linear model is used to reduce 
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Figure 4. Typical Track Generated By A Single Array (X vs Z). 


the random noise in the already calculated X, Y, and Z position coordinates. lhe opti- 
mal Fixed Interval Smoothing algorithm is then used to improve the track quality even 
further. This method is used because it lends itself more readily to post data analvsis. 
The program is written in. Fortran, compiled using the Microsoft, Inc. 4.01 Optimal 
Compiler and run on an IBM-AT personal computer. Details of the program and results 


obtained will now be discussed. 
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Figure 5. Track Generated By Two Arrays In An Overlap Region (X vs Y). 
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Figure 6. Track Generated By Two Arrays In An Overlap Region (X vs Z). 


Il. KALMAN FILTER 


The Kalman Filter is designed to remove random noise from the estimate of the 
parameter with which it is concerned by adding a weighted error term to the observed 
parameter being estimated. The error term is simply the difference between the filter’s 
prediction of the parameter and the actual value observed at a particular time. The 
Weighting factor is influenced by the magnitude of the measurement error and the 
covariance of error between the estimate and the observation. The covariance of error 
of the estimate is also updated based on previous values of the covariance of error. 
Manv treatments of the derivation and application of Kalman Filters were found helpful 
in preparing this report [Refs. 10, 11, and 12], and the equations used in this application 
will now be presented. 

The equations shown below were obtained directly from previous work on this sub- 
ject [Ref. 9: p. 19]. Their development will not be repeated here as an excellent deriva- 


tion of these equations is presented in [Ref. 13: pp. 176-182]. 


£(k 1) 2 (| & — 1) - G(E)Lz() — Z(k | k — 1)] (2.1) 
Х(4--114) -ФХ( 15) | (2.2) 

£(k| k— 1) e H&(K| k — 1) (2.3) 

G(k) 2 P(k|k — DH'LHP(k| k— DH! 4 Ry! (229) 
РК + 1|К) = ФР(К|Юф + ОСЕ) (2.5) 

P(k | К)=[1— СПОНЈР(К | К — 1) (2.6) 


where: 
x- state estimate vector, 
2- observation vector, 
H - measurement matrix, 
ф- state transition matrix, 
G - Kalman gain matrix, 


Р - covariance of estimate error matrix, 


К - covariance of measurement error matrix, 
Q - covariance of excitation error matrix, and 


I - identity matrix. 


In this application, six states were used; namely, the x, y, and z positons and the 
corresponding velocities, x , y, and z. The model for the system, represented by the state 


transition matrix $, is a second order linear model 


(2.7) 


© & с — = 1 
Фон чуо о 


F 
l 

0 
0 
0 
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S d o ooge 
о с) m 
= сэу © © © © 


which when multiplied by the state matrix x(k | k) results in the equations of motion 


x(k + 1) = ХК) + x(k)T (2.8) 


КЕ + D= ST (2.9) 
Z(k + 1) =2(k) +2(k)T (2.10) 


where T represents the time in seconds between samples. Since the parameters being 
estimated are already expressed in a linear coordinate system, the measurement matrix 
H necessary in this case need only extract the observable states, i.e., X, y, and z. The 


measurement matrix used is: 
1 
Н=\0 0. 1 0 0 0 (2.11) 
0 


The values of Q(k) and R chosen play an important role in the performance of the 
filter. The covariance of excitation error, Q(k), is a measure of how confident the filter 


is in the adequacy of the system model together with how strong the noise affecting the 


actual svstem is expected to be. A relatively large value of Q(k) results in the filter 
placing more emphasis on the observation and less on the predicted value when updating 
the estimate, while a smaller value of Q(k) has the opposite effect. Therefore, with a 
larger Q(k), the filter output should be noisier but better able to handle disturbances not 
related to noise such as, in this case, actual target maneuvers. On the other hand, the 
covariance of measurement error R indicates the confidence in the accuracy of the data 
measurements made. It turns out that increasing the magnitude of R in effect decreases 
the gain resulting in less weight being given to the difference between the predicted esti- 
mate and the actually observed value. This results in less attention being paid to the 
noise observed, which makes sense if it 1s assumed that the measured values are less ac- 
curate. [Ref. 12: p. 224] 


The covariance of excitation error matrix used for this filter application 15 as follows: 





= 10 0 0 0 0 0 

0 — 0 0 0 0 

0 0 7 W 0 0 0 
О = (2.12) 

0 0 0 TR 0 0 

0 0 0 0 Түр 0 

4 
0 0 0 0 0 ТО 
1 
where: 

| 0; = в, = с? = E[ ww! ] (2.15) 


A detailed derivation of this matrix is shown in [Ref. 4: pp. 36-42]. Since the excitation 
errors are assumed to be uncorrelated, only the diagonal terms of this matrix are non- 
zero. The value of W is set bv the user while running the program. The value of R 
chosen is 25 square feet. This is a conservative value based on range accuracy estimates 
reported Бру som зе р. О 


Finally, in initializing the filter, an initial value for the covariance of estimate error 
POK K) must be chosen. This value affects the transient response of the filter early on, 
but does not affect the steady state response [Ref. 12: p. 224]. For this filter, P(k k) in- 
itially is set at one million square feet. Again, uncorrelated errors were assumed, so, onlv 
the main diagonal terms are non-zero. The initial value of P(k/k) used here was arrived 
at mainly through trial and error. 

In summary, the Kalman Filter 1s a linear minimum variance estimator whose out- 
put is nothing more than the conditional mean of the parameter being estimated based 
on the observations made. A more detailed description of the actual programs can be 
found in Appendix A and Appendix B. 

This program was run on a typical data file obtained from information provided by 
the Keyport Range. Figures 3 and 4 show the raw track in X vs. Y and X vs. Z coor- 
dinates respectively. Figure 7 on page 12 shows the Kalman Filter variance of the esti- 
mate of the X coordinate of the target’s track. It can be seen that the variance settles 
quickly to a steady state value of about five square feet. The added uncertainty, intro- 
duced by the fact that periodically samples are missed by the range tracking system, 1s 
evident in that the values of P(k'k) do not settle to a constant value, but this uncertainty 
is absorbed by the filter and causes no problem in the filter's performance. 

Figure $ on page 13 shows the Kalman filtered track for the coordinates X vs. Y. 
Clearly, the filtered track is much smoother than the raw data and follows the raw track 
closely after the filter settles out. However, the filter tracks a little low after the target 
appears to make a sharp maneuver to its left because the value of Q(k) 1s small. In other 
words, a small value of Q(k) does not allow the filter to follow maneuvers well, and this 
raw data set looks like it is maneuvering initially. Figure 9 on page 14 15 the same data 
as Figure 8 on page 13 except that the Z coordinates are plotted against the X coordi- 
nates. This plot does not exhibit the same track off behavior because no maneuvers are 
apparent and, in fact, a great deal of noise reduction 1s shown. 

Another run of the program was made using a value of Q(k) increased by a factor 
of 100. Figure 10 on page 15 shows the variance of x and, as expected, it follows the 
same pattern but settles at a higher "steady state" value of approximately 11 square feet. 
Also, the jumps in P(k'k) due to the missed samples are more pronounced. Figure 1l 
on page 16 is a plot of X vs. Y and, when compared to Figure 8 on page 13, clearly 
exhibits the expected behavior. The filtered track is much noisier, but it does not track 
off as before. Figure 12 on page 17 displays X vs. Z and again is a noisier track which 


follows the raw data more closely. 
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Figure 7. Kalman Filter Variance Of X Estimate (Small Q(k)). 


The Kalman Filter routine designed for this application provides the user with a fil- 
tered set of data points which are an linprovement over the actual raw data available in 
that, as expected, much of the random noise which tends to corrupt the raw data has 
been eliminated. Proper selection of the R and Q(k) matricies and the initial value of 
P(kK/k) ensures that the filter 1s receptive to target maneuvers while at the same time 
significantly reducing the random noise. The user must decide on the proper balance to 
fit his needs based on known information such as the expected maneuverability of the 
target and the magnitude of the expected measurement errors. In addition, this filtered 


data is now ready to be processed by the Fixed Interval Smoothing Routine. 


| 
D 
к= 
~ 
= 


— 
EM 
= 
= 
(2 
г. 
< 
Са 
ил 
2 
о 
Со 
о 


2.29 2.3 


DOWNRANGE (ЕТ) 





Figure 8. Kalman Filtered Track - X vs. Y (Small Q(k)). 
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Figure 9. Kalman Filtered Track - X vs. Z (Small Q(k)). 
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Наше 10. Kalman Filtered Variance Of X Estimate (Large Q(k)). 
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Figure 11. Kalman Filtered Track - X vs. Y (Large Q(K)). 


— 
E 
= 
м“ 
ае 
= 
Qu 
а 
а 


2.15 a.c oO 2.9 2.35 2.4 


DOWNRANGE (FT) x104 





Figure 12. Kalman Filtered Track - X vs. Z (Large О(К)). 


ПІ. FIXED INTERVAL SMOOTHING ALGORITHM. 


The Fixed Interval Smoothing Algorithm is one of the three algorithms designed to 
improve upon the Kalman Filter’s results by taking into account data which was not 
available when each Kalman Filter estimate was made and updating each previous esti- 
mate accordingly. The Fixed Point and Fixed Lag Smoothing algorithms, while very 
similar to the Fixed Interval Smoothing routine, differ in the way thev include this ad- 
ditional data into each estimate. The Fixed Interval Smoothing algorithm recalculates 
each estimate generated by its associated Kalman Filter based on information obtained 
over the entire interval of data being analvzed. In this sense, it is useful only as a post 
data analysis tool, since the entire set of data over the given interval must be known, and 
Kalman Filter estimates and covariance of error between estimates and observations 
must be generated previously. In implementing this algorithm, a system of recursive 
equations which operate backwards in time from the last data point to the first data 
point are used. 

The equations for the Fixed Interval Smoothing algorithm used in this application 
were obtained from [Ref. 9: p. 20]. Several sources were beneficial in understanding 
these equations [Refs. 10, 11, and 14 ] and [Ref. 13: pp. 216-225] provides an excellent 
derivation of them. Therefore, these derivations will not be repeated here. The 


equations of interest are: 


(А | № = A) + АПОГХ(Е + 1] А) — 2K +1] 4] (3.1) 
AK = РК | Юо’ РКИ Ю À (3.2) 
Р(К | №) = РА) + АГРЕ + 1] А) — РА + 01400” (3.3) 


where: 
x(k | N) - smoothed estimate at sample k, 
P(k | N) - smoothed covariance of estimate error at sample К, 
A(k) - smoothing algorithm gain matrix, and 


x(k | k) and P(k | k) - values stored by the Kalman Filter routine. 


It can be seen from the above equations that the estimate generated by the Fixed 


Interval Smoothing algorithm is simply the Kalman Filter estimate adjusted by a 


weighted error term. The error term is the difference between the smoothed estimate 
calculated for the previous data point (which is actually the next sequential data point 
in the file in real time), and the predicted value of the corresponding parameter gener- 
ated by the Kalman Filter. The gain matrix A(k) is dependent on the covariance of error 
between estimate and observation generated by the Kalman Filter, namely, P(k’/k) and 
P(k+1/k). The smoothed value of the covariance of error P(k/N), while having no im- 
pact on the smoothed estimate x(k | №) , provides a measure of how well the smoothing 
filter is working. In general, P(k/N) should be less than P(k/k) except at the Nth point 
where they should equal one another. As the smoothing filter moves backward in time, 
it adjusts the original Kalman Filter estimate depending on the smoothed estimate’s 
agreement with the predicted value for the previous point operated on, and on the con- 
fidence level of the Kalman Filter in its own solution as indicated by the values of P(k’k) 
and P(k+1/k). Simply stated, if more uncertainty exists in the Kalman Filter Solution, 
more weight is given to the difference between previous smoothed estimates and pre- 
dicted values of the parameter in computing the current smoothed estimate. 

The Fixed Interval Smoothing portion of the program listed in Appendix A imple- 
ments the equations shown above. As with the Kalman Filter portion of the program 
described in the previous chapter. there is no provision for the resultant data to be dis- 
played graphically bv this program itself because the size of the data files involved ex- 
ceeds the capabilities of the plotting routines available. However, graphical results are 
easily obtained from the data files output by the program using Matlab. Details of 
Matlab plotting capabilities and descriptions of the commands used to generate the plots 
shown can be found in [Ref. 15]. 

Results of the smoothing algorithm were obtained using the Kalman Filter results 
presented in the previous chapter as inputs to the smoothing loop of the program. As 
in chapter 2, data is presented in the X vs. Y plane and the X vs. Z plane, and cases were 
run for both values of Q(k). 

Figure 13 on page 20 shows the variance of the x estimate associated with the 
smoothing routine together with the variance of x achieved with the Kalman Filter. As 
expected, the value of the smoothed variance is a definite improvement over the Kalman 
Filter result, and in fact settles to an average value that is less than 1.5 square feet. 
Figure 14 on page 21 is the smoothed version of Figure 8 superimposed on the raw X 
vs. Y track for the case where Q(k) is small and shows a smooth track which follows the 
raw track closely. The divergence seen initially in the filtered results for this case has 


been compensated for, however, a close look at the smoothed results shows some di- 
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Figure 13. Kalman Filtered & Sinoothed Variances Of X (Small Q(k)). 


vergence still present In the first portion of the track. Figure 15 on page 22 is the 
smoothed version of Figure 9 superimposed on its raw data, and again a significant im- 
provement 1s demonstrated. 

The Kalman Filter results of the case where a large Q(k) was used were also treated 
with the smoothing algorithm. Figure 16 on page 23 is the smoothed plot of the vari- 
ance of the x estimate together with the variance of x shown in Figure 10. This plot 
indicates that the output 1s much more influenced by noise and sample uncertainty due 
to the higher sensitivity when compared to the small Q(k) case, but is a great improve- 
ment over the Kalman Filter output. Here the variance settles to an average value of 
approximately five square feet. Figure 17 on page 24 shows the smoothed version of 


Figure 11 superimposed on its raw track. This smoothed track greatly improves on the 
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Figure 14. Raw & Smoothed Tracks - X vs. Y (Small Q(k)). 


filtered track while showing none of the divergence from the raw track which was seen 
in the small Q(k) case. Figure 18 on page 25 is the raw and smoothed X vs. Z tracks 
for the large Q(k) case and, when compared to the Kalman Filter results shown in Figure 
12, clearly illustrates the excellent performance of the smoothing routine. 

These results show that the smoothing algorithm dramatically improves upon the 
results obtained with the Kalman Filter alone. This intuitivelv makes sense because the 
estimates arc now based on the behavior of the entire data set instead of on just what 
Is known about the data at the time the estimate is made. The smoothed results depend 
indirectly upon the values chosen for Q(kK) and P(k/k) initially and can even compensate 


somewhat for poor choices of the parameters in the initialization of the Kalman Filter. 
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Figure 15. Raw & Smoothed Tracks - X vs. Z (Small Q(k). 


These results demonstrate that the Kalman Filter Smoothing Algorithm emploved here 


is an excellent post-data analysis tool. 
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Figure 16. Kalman Filtered & Smoothed Variances Of X (Large О(К)). 
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Figure 17. Raw & Smoothed Tracks - X vs. Y (Large Q(k)). 
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Figure 18. Raw & Smoothed Tracks - X ys. Z (Large О(К)). 
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IV. SOLVING THE ARRAY HANDOFF PROBLEM. 


As discussed in the Introduction, one of the major problems with the tracks gener- 
ated by the current Keyport short base-line tracking array system is the presence of 
discontinuities in array overlap regions. These discontinuities in the target track caused 
by differing bias errors present in each arrav's solution complicate the post data analysis 
so crucial to torpedo testing and evaluation. It can now be demonstrated that the Fixed 
Interval Smoothing Kalman Filter Algorithm, as implemented by the program presented 
here, can improve the overall track quality immensely іп the overlap regions. 

As shown In previous chapters, the Kalman Filter and Fixed Interval Smoothing 
routines effectively remove random noise from the generated track data and generate 
smooth tracks which are easy to see. The processing of track data from two arrays 
where manv sample times have two different values simultaneously is handled quite 
satisfactorily bv the algorithms. The data is treated simply as two distinct samples where 
no time has elapsed between samples. This overlap data is also characterized by the fact 
that many samples are missed and occasionally relatively long periods of time pass be- 
tween data points. It will be seen that this problem 1s also handled adequately by the 
algorithms. Another problem experienced with these data sets was that they were too 
large to be handled бу the personal computer's compiler. This problem was solved Бу 
switching to the Microsoft, Inc. 4.01 Optimizing Compiler. This compiler not only 
compiled the program using less memory, but use of the SLARGE metacommand al- 
lowed the larger data sets to be processed. Detailed descriptions of this compiler and the 
available memory models can be found in [Ref. 16]. Two sets of overlap data were an- 
alvzed and the results of this analysis are now presented. 

Figure 19 on page 27 1s a plot of the Kalman Filter variance of the x coordinate 
estimate vs. time, together with the smoothed variance of the x estimate for the first set 
of overlap data processed. It is seen that the filtered variance drops quickly to a steady 
state value of about 6.5 square feet, and then jumps back up erratically over the last 
third of the set. The thick portion of the filter's variance is where overlap is occurring 
because here two values of the parameter are available for each time, so, two steady state 
values of the variance of the estimate are consistently reached. The erratic values of the 
filter's variance result from the fact that over this region the sample interval varies er- 


ratically as samples were missed, so, the uncertainty of the filter is also affected. The 
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Figure 19. Kalman Filtered & Smoothed Variances Of X (Arrays 1 & 11). 


smoothing algorithm's variance of the x estimate 1s, as expected, a significant improve- 
ment over the filter’s performance alone. Note that the overlap region now settles to a 
single steady state variance of about 2.5 square feet, and that, although the variance 
jumps up over the latter portion of the data set as before, the values are again much 
smaller than the filtered result. Figure 20 on page 28 displays the Kalman filtered track 
of the data shown in Figure 5 for the X vs. Y coordinates case. The filtered track over- 
shoots the raw track initially, indicating that better tuning may be required as discussed 
earlier, but then settles to an average of the two distinct array tracks throughout the 
overlap region. Finally, the filtered track closely follows the track after the overlap re- 
gion has ended while effectively removing much of the random noise present. 


Figure 21 on page 29 is the smoothed version of l'igure 20 on page 28, and proves to 
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Figure 20. Kalman Filtered Track - X vs. Y (Arrays ] & 11). 


be an easy-to-see track which does not overshoot the target's maneuver. Thus, the 
smoothing routine has not only smoothed the filtered track considerablv, but it has also 
compensated for the possible tuning problem. Figure 22 on page 30 is the smoothed 
track superimposed on the raw data, and it shows this clearly. Figure 23 on page 31 and 
Figure 24 on page 32 show the filtered and smoothed tracks respectively for the X vs. 
Z case. These plots exhibit the same characteristics as the X vs. Y plots discussed above 
as expected. Figure 25 on page 33 highlights the overall improvement gained by 
smoothing the raw data. 

Another set of data which contained an overlap region was processed by the pro- 
gram. This data set was considerably larger than the previous case but was still within 


the capacity of the personal computer. This data is illustrated in Figure 26 on page 34 
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Figure 21. Smoothed Track - X vs. Y (Arrays 1 & 11). 


and Figure 27 on page 35. Figure 28 on page 36 displays the filtered and smoothed 
values of the x estimate variances vs. time. As in the previous case, the missed samples 
and overlap region are apparent from the filtered results, and the smoothed results show 
much improvement. Figure 29 on page 37 shows the filtered results while Figure 30 on 
page 38 shows the smoothed results of the X vs. Y tracks for the arrays 2 & 12 overlap 
region, and it is clearly illustrated that the track quality is again vastly improved. 
Figure 31 on page 39 is the smoothed X vs. Y track superimposed on the raw data 
demonstrating that the smoothed data does indeed follow the raw data closely. 
Figure 32 on page 40 and Figure 33 on page 41 show the filtered and smoothed X vs. 
Z plots respectively with equally good results. Finally, Figure 34 on page 42 15 also in- 


cluded to highlight the final results of the program. Notice that there 1s no overshoot 
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Figure 22. Raw & Smoothed Tracks - X vs. Y (Arrays 1 & 11). 


in these filtered tracks as was seen in the previous case, indicating that the Kalman Filter 
was properly tuned for this run. 

These results clearly demonstrate that the Kalman Filter with the Fixed Interval 
Smoothing Algorithm will essentially “solve” the array handoff problem by removing 
random noise and taking on a single average value through overlap regions based on 
information obtained from all hydrophones in question. The program effectively han- 
dles missed points and samples with multiple data especially where the the smoothed 
output is considered. Therefore, with sufficient computer memory available, this pro- 
gram could accept a target’s entire range track file filled with missed samples and over- 


lapping data regions and produce the optimum track for the known information. This 


30 


2-9 


– 330 | 


– 340 


– 350 | 


– 360 


– 370 


– 380 
– 390 | 
– 400 
-410) 


1.8 2 2.2 2.4 2.6 2.8 3 


DOWNRANGE (ЕТ) x104 





Figure 23. Kalman Filtered Track - X vs. Z (Arrays 1 & 11). 


fact makes this an extremely useful post-data analysis tool for evaluating typical range 


generated tracks. 
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Figure 24. Smoothed Track - X vs. Z (Arrays 1 & 11) 
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Raw & Smoothed Tracks - X vs. Z (Arrays 1 & 11). 


~ 
(л 


Figure 


33 


ы 
Е- 
= 
2% 
O 
Z, 
< 
ба 
07 
02 
© 
ёо 
O 


3 


DOWNRANGE (FT) 





Figure 26. Raw Track - X vs. Y (Arrays 2 & 12). 
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Figure 27. Raw Track - X vs. Z (Arrays 2 & 12). 
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Kalman Filtered & Smoothed Variances Of X (Arrays 2 & 12). 


Figure 28. 
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Figure 29. Kalman Filtered Track - X vs. Y (Arrays 2 & 12). 
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Figure 30. Smoothed Track - X vs. Y (Arrays 2 & 12). 
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Figure 31. Raw & Smoothed Tracks - X vs. Y (Arrays 2 & 12). 
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Figure 32. Kalman Filtered Track - X vs. Z (Arrays 2 & 12). 
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Figure 33. Smoothed Track - X vs. Z (Arrays 2 & 12). 
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Figure 34. Raw & Smoothed Tracks - X vs. Z (Arrays 2 & 12). 
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V. CONCLUSION 


The results above have demonstrated the effectiveness of this Kalman Filter Fixed 
Interval Smoothing Algorithm in smoothing a target track for use in post-data analysis. 
A raw target track characterized by discontinuities in array overlap regions and random 
noise throughout can easily be transformed into the optimal track given the known in- 
formation. All the user need do is set up an appropriate input file consisting of sample 
count and X, Y, and Z coordinates. The program listed in Appendix B is designed to 
take current data files from the Keyport Torpedo Station and reformat them into suit- 
able input files for the main smoothing program using data from any two arrays. This 
program could easily be modified to include data from as many arrays as desired. Thus, 
the original goal of this research, to provide a smoothing algorithm tested on actual 
range data which effectively deals with the array handoff problem, has been successfully 
achieved. In addition, this algorithm’s success suggests some other interesting possibil- 
ities for improving short base-line array tracking range capabilities. 

Another possible use for this algorithm is range calibration. As has been discussed, 
the current tracks generated bv the short base-line arrays contain random errors and bias 
errors due to a myriad of sources. Ifa test craft could be tracked accurately independent 
of the acoustic range, it could be used to generate a true track for comparison to the 
smoothed track produced by this program. It follows that since the Kalman Filter 
Smoothing routine effectively reduces random errors while having no effect on bias er- 
rors, any bias errors present in the smoothed track would be highlighted; whereas, they 
may have been lost before in the random noise. These bias errors could then be ac- 
counted for in the software that calculates the target’s position. Calibration runs could 
be made as frequently as desired, and in this way, bias errors could be promptly and 
easily compensated for. 

The Kalman Filter portion of the program presented here is designed to operate on 
a file of existing data points. However, it would be a relatively simple matter to alter this 
program to read one data point, operate on the data, generate a filtered data point, and 
store the required information for use in the smoothing portion of the program. The 
smoothing routine in this program is desired to run after all the data has been Kalman 
filtered, so, no changes are required in this portion of the program as long as the last 


data point of the track is properly flagged. The implications of these facts is that this 
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program could be used to improve the real time tracks generated by the range as well 
as providing a smoothed track after the run is complete for post-data analysis and range 
calibration. The program need only be given the computed output of the tracking 
computer and then allowed to supply its filtered result to the algorithm which generates 
the track displavs. Furthermore, improved tracking accuracy could be obtained using 
an adaptive excitation matrix scheme. It was shown earlier how Q(k) can affect the fil- 
tered results. Assuming frequency information could be provided to the program in real 
time, the magnitude of Q(k) could be altered in response to doppler shifts in received 
frequency. In other words, if a target maneuver is indicated by а doppler shift, the 
magnitude of Q(k) is increased to allow the filter to track through the maneuver. Once 
the doppler information indicates that the target is no longer maneuvering, Q(k) is de- 
creased to smooth out the filtered track. In this way, the current real time tracking ca- 
pabilities of these ranges could be improved with a reasonable amount of changes in the 
tracking software required. 

In conclusion, the approach used here of Kalman Filtering the target’s computed 
position in X, Y, and Z coordinates is considered to be successful. This method is an 
effective compromise between the theory of optimal linear estimation and the opera- 
tionally oriented user who may be leery of trusting massaged data, because the raw data 
is not lost. Therefore, the user can always make comparisons between raw and treated 
data. In addition, this program is seen as flexible enough to be applied to a number of 
possible applications concerning the existing short base-line array tracking ranges. 


Finally, it is an excellent post-data analysis tool in its present form. 


APPENDIX A. KALMAN FILTER FIXED INTERVAL SMOOTHING 
ALGORITHM 


A. MAIN PROGRAM 

A brief description of the main program and subroutines is given. Initially, the 
program reads in data from a user-specified data file and sets a point counter represented 
by the variable IRI. After initialization of the necessary variables, the program deter- 
mines the time slot to the next sequential data point in the file and converts it to seconds 
for use by the subroutine which calculates the state transition matrix @. Next, the 
excitation matrix Q(k) 1s computed. Notice that this calculation requires the time 
elapsed from the last sample. The gain matrix G(k) is then computed followed by the 
filtered estimates of the states. The program then stores the filtered estimates along with 
the information needed for the smoothing algorithm, and then initializes the array for 
later storage of the smoothed estimates. The variance of the x estimate is sent to a 
subroutine to be placed in a data file. 

The Fixed Interval Smoothing portion of the program first sets up its loop counter 
to count back from the filtered loop endpoint. Next, ó is recomputed using times be- 
tween samples that are consistent with the Kalman Filter case. Pertinent information 
stored in the Kalman Filter loop is recovered, and the smoothed estimates are then 
solved for. The smoothed x estimate variance is then sent to a subroutine which includes 
it in an output file, and finally, the filtered and smoothed estimates are written to output 
data files. 


B. SUBROUTINES 


• РГОТРККЗ : 
outputs the variance of the x estimate vs. discrete time interval for the Kalman 
Filtered case and the Smoothed case 


e PHIDEL | | 
computes the state transition matrix based on time between samples using a 
linear second order model 


° GAIN | 
computes P(k/k-1),P(k’k), and the gain matrix G(k) 


e ADD 
adds two input matrices 


° SUB | 
subtracts second input matrix from first 
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° PROD 
multiplies two Input matrices 


° TRANS 
calculates the transpose of the matrix Input 


° RECIP 
algorithm taken from [Ref. 17: p. 163] which inverts the input matrix 


° INITS 
Initializes matrices at the beginning of the main program 


° CHANGE 
allows user to change the magnitude of the excitation matrix Q(k) and the ini- 
tial value of the estimate's covariance of error matrix P(k/k) 


SLARGE 

C---KALMAN FILTER---KALMAN SMOOTHING ALGORITHM----------------- 
C---THIS PROGRAM IS DESIGNED TO PROCESS 3/D DATA FILES FROM 

C---THE UNDERSEA TRACKING RANGE AT KEYPORT, WA. A KALMAN FILTER 
C---IS APPLIED TO THE TRACK DATA WHICH CONSISTS OF X,Y, AND Z 
C---COORDINATES. THEN, A KALMAN FILTER SMOOTHING ROUTINE GENERATES 
C---SMOOTHED POINTS IN X,Y, AND Z. THE PROGRAM GENERATES OUTPUT 
C---FILES WHICH CONTAIN THE VARIANCE OF THE X ESTIMATE VS DISCRETE TIME 
C---FOR BOTH THE FORWARD KALMAN FILTER CASE AND THE KALMAN SMOOTHED 
C---CASE. FILES ARE ALSO GENERATED WHICH CONTAIN THE FILTERED 
C---X,Y,AND Z ESTIMATES AND THE SMOOTHED X,Y,AND Z ESTIMATES. 
C---THIS PROGRAM IS DESIGNED TO RUN ON THE IBM/AT PERSONAL 
C---COMPUTER BUT DUE TO THE SIZE OF THE DATA SETS INVOLVED, PLOTTING 
C---CANNOT BE DONE WITH THIS PROGRAM.  PLOTTING OF OUTPUT DATA IS 
C---DONE USING MATLAB. THE PROGRAM GIVES THE USER THE OPTION OF 
C---CHANGING THE VALUE OF THE INITIAL COVARIANCE MATRIX AND THE 
C---EXCITATION PROCESS VECTOR. THE USER IS ALSO REQUIRED TO PROVIDE 
C---THE NAMES OF THE INPUT AND OUTPUT DATA FILES. 


C 
C wwie DECLARATION OF VARIABLES зе 
C 
COMMON W(6,6),XKK(6),PHI(6,6),Q(6,6),XKKM1(6), 
* A(6,6),B(6,6),H(6,6),HI(6,6) ,PKKM1(6,6),PKK(6,6) 
COMMON /CBLK/ RAW(3,1000),FILT(3,1000) ,SMOOTH(3,1000) ,PTC(1000), 
* XYRANGE( 3,2) , PKKONEONE( 1000) , IR1 
CHARACTER INFIL*13, FILTER*13, OUTFIL*13 
DIMENSION R(6,6),G(6,6),XRR( 1000) , YRR( 1000) , ZRR( 1000) , PHIT(6,6), 
*77(6),E(6),GE(6) , PKKS3D(6,6, 1000) , PKKM1S(6,6, 1000) ,XKKS(6,1000), 
*C(2,2),D(2,2),PKKS(6,6),P2(6,6),PNNM1(6,6), ZKKM1(6) , AKT(6,6), 
*SS3(6,6) ,SS3R(6,6),AK(6,6) ,XNNM1(6) ,SS2(6),XP1(6),TEMP4(6,6), 
*TEMP5(6,6) ,TEMP6(6,6), TEMP3(6,6) , TEMP2(6), TEMP1(6,6) , TMP(6) 
C 
C wee  INPUT DATA, DESIGNATE FILENAMES "eee 
C 


WRITE(*,*) ‘ENTER NAME OF INPUT DATA FILE' 
READ(*,'(A)') INFIL 

WRITE(*,*) 'ENTER NAME OF FILTERED DATA FILE' 
READ(*,'(A)') FILTER 

WRITE(*,*) 'ENTER NAME OF SMOOTHED DATA FILE' 
READ(*,'(A)') OUTFIL 
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оо 


С2 С2С<2Сс2С2 сз Су С) 


aaa 


15 


20 


100 


BBENGNSEILE- INFIL,STATUS- 'OLD') 

OPEN(2,FILE= FILTER,STATUS= 'NEW' ) 

OPEN(3,FILE= OUTFIL,STATUS= 'NEW' ) 

Eg 

READ(1,*,END-220)PTC( IR1),XRR(IR1),YRR(IR1),ZRR(IR1) 
IR1=IR1+1 

GOTO 15 

CONTINUE 

IR1 = IR1 - 1 


хээ INITIALIZE VARIABLES AND DATA ARRAYS www* 


L 
M 
N 
MD 
ND 
LD 
DT 
= 
W(1,1) 
RET 
R( 2,2) 
К(5 3) 
R(4,4) 
R5 75) 25 
R(6,6) 25 
CALL INITS 


1 
3 
6 


нии 


I H H I 


Оо O O. ON O. 


. 0001 
25 
25 
25 
25 


xxx% CHECK INITIAL PARAMETERS “ки 


CALL CHANGE(W, PKKM1) 
PDT TERETE TERETE LEDERER EES REET 


KALMAN FILTER ROUTINE 


4 68 48 4v 4v 4 £V 4S 6S 4S VV £N 4S £8 48 4S 48 23 60 40 40 00 43 60 0) 48 00 00 09 65 


хээ GENERATE PHI AND Q MATRICES www 


ЇЕ КГ ЕС. ІКІ) THEN 


DT = 2. 666 
ELSE 

DT = 1.333*(PTC(K*1) - PTC(K)) 
END IF 


CALL PHIDEL(DT,N,A,PHI) 
ШЕК ЕО: 1) THEN 


DT = 2.666 
ELSE 

DT 2 1.333*(PTC(K)- PTC(K-1)) 
END IF 
Q(1,1) 2 (DT**4/4) * W(1,1) 
0022) ps2 * WC1, 1) 
фен 0(1,1) 


Q(^,4) - 0(2,2) 
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CQ): 2 C С 


са сз са са 


C2 C2 Ga 


сос с а 


40 


50 


60 


501 


O o, 5) e= 04 
O 626) em 
RAW(1,K) = XRR(K) 
RAW(2,K) = YRR(K) 


RAW(3,K) = ZRR(K) 

+ CALCULATE GAIN MATRIX AND SOLVE FOR К 
хөвж ХСК/К) = Х(К/К-1) + С(К)*[2(К) - 2(К/К-1)] ж 
жижи WHERE Z(K) IS THE OBSERVABLE хэсэх 


CALL GAIN( PKK, PKKM1,Q,R,PHI,H,N,G,HI,ND,MD,LD) 
CALL PROD( PHI ,XKK,N,N,1,XKKM1,ND,MD, LD) 

CALL PROD(H,XKKM1,N,N,1,ZKKM1,ND,MD,MD) 
22(1)-ХВЕ(К) 

ZZ(2)=YRR(K) 

ZZ(3)=ZRR(K) 

CALL SUB(ZZ,ZKKM1,M,1,E,ND,MD) 

CALL PROD(G,E,N,M,1,GE,ND,MD,LD) 

CALL ADD(XKKM1,GE,N,1,XKK,ND,MD) 


ххх" ~= STORE X(K/K) ,P(K/K),P(K/K-1), INITIALIZE THE SMOOTHED хх» 


Jrk DATA ARRAY, AND STORE THE FILTERED ESTIMATES. 


DO 40 I = 1,6 
DO 40 J = 1,6 


PKKS3D(1,J,K) = PKK(I,J) 
PKKMIS(I,J,K) = РККМ1(1,3) 
DO 50 I=1,6 
РАНИ КОК) Кот) 
DO 60 I = 1,3 
FILT(1,K) = XKK(2*I-1) 


SMOOTH(I,K) = FILT(I,K) 
PKKONEONE(K) = PKKS3D(1,1,K) 
Тате сато 100 
CTS 
CALL PLOTPKKS(CT) 


ОУ ОУ ОИ 


KALMAN SMOOTHING ROUTINE 


С је је Ја 
9% 9% Фу Фо Ф\ éA 4S 08 06S 4S 4 48 6» 0» 6» 09 068 6» бу 0» 09 dork eR ev Ed Ed Fd db ed 609 2“ 


DO 600 K=1,IR1 - 1 
KI= IR1 - K 


хэл GENERATE PHI MATRIX AND RETRIEVE STORED X(K/K), 


"UU 5 P(K,K), AND P(K+1/K) 


DT = 1.333*(PTC(KI+1) - PTC(KI)) 
CALL PHIDEL(DT,N,A, PHI) 
DO 501 I = 1,6 
XP1(I) = XKKS(I,KI) 
CONTINUE 
DO 502 I = 1,6 
DO 502 J = 1,6 
P2(I,J) = PKKS3D(I,J,KI) 
653(І,7) - РККМ15(1,7,КІ) 
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не 


Hedy 
Yves 


C)cyc O 


©) Ci 


Соса со Су Су СУС 


502 CONTINUE 


ээс SOLVE FOR SMOOTHED ESTIMATE к 
wz Х(К/М) = Х(К/К) + А(К)*[ Х(К+1/М) - Х(К+1/К)] иж 


CALL TRANS(PHI,N,N,PHIT,ND,MD) 
CALL RECIP(SS3,SS3R,N) 
CALL PROD(PHIT,SS3R,N,N,N,TEMP1,ND,MD,LD) 
CALL PROD(P2,TEMP1,N,N,N,AK,ND,MD,MD) 
DO 504 I = 1,6 
XNNM1(I) = XKKS(I,KI+1) 
504 CONTINUE 
CALL PROD(PHI,XP1,N,N,L,SS2,ND,MD,L) 
CALL SUB(XNNM1,SS2,N,1,TEMP2,ND,L) 
CALL PROD(AK,TEMP2,N,N,1,TEMP3,ND,MD,L) 
CALL ADD(XP1,TEMP3,N, 1, TMP,ND,MD) 
DO 505 I = 1,6 
505 ХККЅ(І,КІ) = ТМР(І) 
DO 506 I = 1,6 
DO 506 J = 1,6 
PNNM1(I,J) 9 PKKS3D(I,J,KI*1) 
506 CONTINUE 
CALL SUB(PNNM1,SS3,N,6,TEMP4,ND,MD) 
CALL TRANS(AK,N,N,AKT,ND,MD) 
CALL PROD(TEMP4,AKT,N,N,N, TEMP5 ,ND,MD,MD) 
CALL PROD(AK,TEMPS5,N,N,6, TEMP6 ,ND,MD,MD) 
CALL ADD(P2,TEMP6,N,6,PKKS,ND,MD) 
DO 508 I = 1,6 
DO 508 J = 1,6 
508 PKKS3D(I,J,KI) = PKKS(I,J) 


живи STORE X(K/K),P(K/K),AND OUTPUT SMOOTHED DATA з 


DO 520 I = 1,3 
520 5М00ТН(І,КІ) - ХКК5(2%1-1,КІ) 
PKKONEONE(KI) = PKKS3D(1,1,KI) 
600 CONTINUE 
CT = 0 
CALL PLOTPKKS(CT) 
DO 650 K = 1,IR1 
WRITE(2 ,*)(FILT(I,K),I-1,3) 
WRITE(3,*)(SMOOTH(I,K),I21,3) 
650 CONTINUE 
END 


Зее dese eyes es ese ve ev es ese ees eec ev ee eye yes es eye dese yes eve К ККЖ 


SUBPROUTINE WHICH OUTPUTS THE VARIANCE OF THE X ESTIMATE VS 
DISCRETE TIME FOR BOTH THE FILTERED CASE AND THE SMOOTHED CASE 


veveveveveveveve vede dev eyede vedete de ToT TOTES Vedeve dede vede ede ede deed ee dee ее СКУКИ 


SUBROUTINE PLOTPKKS(CT) 

COMMON /CBLK/ RAW(3,1000),FILT(3,1000),SMOOTH(3, 1000) , PTC( 1000), 
* XYRANGE( 3,2) , PKKONEONE( 1000) , IR1 

DIMENSION TK(1000) 

CHARACTER FWDVAR3*13, BKVAR*13 
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Се? 


СО СО СОО СО ООО c> C СОО С СОЭ 


IF (CT .EQ. 1) THEN 
WRITE(*,*) ‘ENTER NAME OF FILTERED VARIANCE FILE' 
READ(*,'(A)') FWDVAR 
OPEN(4,FILE= FWDVAR, STATUS= 'NEW' ) 

ELSE 
WRITE(*,*) ‘ENTER NAME OF SMOOTHED VARIANCE FILE' 
READ(*,'(A)') BKVAR 
OPEN(5,FILE= BKVAR, STATUS= 'NEW') 

END IF 

DO 10 Т=1 ШЕ] 

ТК(1)РТС(1)-РТС(1) 

IF (CT .EQ. 1) THEN 
WRITE(4,*)TK(I),PKKONEONE(I) 

ELSE 
WRITE(5,*)TK(I),PKKONEONE( I) 

END IF 

10 CONTINUE 

RETURN 

END 


еее kk kkrk ce 


SUBROUTINE WHICH COMPUTES THE PHI MATRIX 
PHI = I + A*T 


у СУ У У У ле и У ехо и УЛУК 


SUBROUTINE PHIDEL(T,N,A, PHI) 
DIMENSION A(6,6),PHI(6,6),COR(6,6),C(6,6) 


F-1. 
DO 100 IR = 1,N 
DOREM NN 
PHI(IR,IC) = 0. 
PHI(IR,IR) = 1. 
100 С(ІБ,ІС) = ACIR,IC) 
110 DO 120 IR = 1,N 
DO 120 IC = 1,N 
СОВ(ІВ,ІС) = T/F*C(IR,IC) 
120  PHI(IR,IC) = PHI(IR,IC)+COR(IR, IC) 
RETURN 
END 


ес 


THIS SUBROUTINE COMPUTES THE OPTIMUM GAIN MATRIX G(K) AND 
THE COVARIANCE MATRIX P(K+1/K) BASED ON THE EQUATIONS: 


G(K) = P(K/K-1)*TRANS°H(K) *INV°[H(K)*P(K/K-1)*TRANS°H(K) + R] 


P(K/K+1) = PHI*P(K/K)*TRANS°PHI + (К) 
ИКИ К 


SUBROUTINE GAIN( PKK, PKKM1,Q,R,PHI,H,N,G,HI,ND,MD, LD) 
П1ЇМЕМ5ТОМ РККС6,6),9(6,6),НС6,6),509 0 8 2006 6» 66). 
% НІ(6,6),НТ(6,6),ТЕМР(6,6),ТЕМР1(6,6),ТЕМР2(6,6), 
* PHI(6,6),PHIT(6,6),PKKM1(6,6) , TEMP3(6,6) , TEMP4(6,6) 

CALL TRANS(H,N,N,HT,ND,MD) 

CALL PROD( PKKM1,HT,N,N,N, TEMP ,ND,MD, LD) 

CALL PROD(H,TEMP,N,N,N,TEMP1,ND,MD,LD) 
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cC, OQ O O O GD: CO G СООСУ 


тро ФФ 


DO 140 I=1,N 
DO 140 J=1,N 

140  TEMP3(I,J)-TEMP1(I,J) 
TEMP3(1,1)=TEMP1(1,1)+R(1,1) 
TEMP3(2,2)=TEMP1(2,2)+R(2,2) 
TEMP3(3,3)=TEMP1(3,3)+R(3,3) 
TEMP3(4,4)=TEMP1(4,4)+R(4,4) 
TEMP3(5,5)=TEMP1(5,5)+R(5,5) 
ТЕМРЗ(6,6)=ТЕМР1(6,6)+8(6,6) 
DO 143 I=1,N 
DO 143 J=1,N 

143  С(1,3)-ТЕМРЗ(1,2) 
CALL RECIP(C,D,N) 

170 CALL PROD(TEMP,D,N,N,N,G,ND,MD,LD) 
CALL PROD(G,H,N,N,N,TEMP,ND,MD,LD) 
DO 180 I = 1,6 
DO 180 J = 1,6 

180 ТЕМР(І,7) = -TEMP(I,J) 
CALL ADD(HI,TEMP,N,N,TEMP,ND,MD) 
CALL PROD( TEMP, PKKM1,N,N,N, PKK,ND,MD, LD) 
CALL TRANS(PHI,N,N,PHIT,ND,MD) 
CALL PROD(PKK,PHIT,N,N,N,TEMP,ND,MD,LD) 
CALL PROD(PHI,TEMP,N,N,N,TEMP1,ND,MD, LD) 
CALL ADD(TEMP1,Q,N,N,PKKM1,ND,MD) 
RETURN 
END 


а И а XUCRUKUX CKCKIXIX KX XX<<292<X XX KuKCKCKG 


SUBROUTINE WHICH ADDS TWO INPUT MATRICES 


VV (V EVEN VN YN HN EN eV VV VV YS VN ON OV VN VV eV VV VS ev eV vv ev YN VV VC VV uv (V Vv vv 2v VC av eS 2v Vv ov ov el o vv 26 ev 2S QU uv ev 2v de uv vv eo vv vv 2€ eC 


SUBROUTINE ADD(A,B,N,M,C,ND,MD) 
DIMENSION A(ND,MD) , B(ND, MD) , C(ND,MD) 
DO 100 I = 1,N 
DO 100 J 
ШОН J) 
RETURN 
END 


1,M 
Cit ae Ged 


еее плети Fe BRIER ERG: 


SUBROUTINE WHICH SUBTRACTS THE SECOND INPUT MATRIX FROM THE FIRST 


шеек рее е gua F ro * + Ж, 4 $ Ты! 5. ~". ~", у у у аы ае э ә мы Поов гејева ја пђо по 
Өлултулллтутуллууллллллулллллїллллїллїүулллулулултлулл,уХлт2ТїуХХУУТТЛОН 


SUBROUTINE SUB(A,B,N,M,C,ND,MD) 
DIMENSION A(ND,MD),B(ND,MD) ,C(ND,MD) 
DO 100 I = 1,N 
DO 100 J = 1,M 
100 C(I,J) » A(I,J) - B(I,J) 
RETURN 
END 


хил их кип лж 
SUBROUTINE WHICH MULTIPLIES TWO INPUT MATRICES 


U = ~ a = 
еее еек еее dedeevededese 


SUBROUTINE PROD(A,B,N,M,L,C,ND,MD,LD) 
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И + C A 


са са сос са 


DIMENSION A(ND,MD),B(MD,LD),C(ND,LD) 
DO 100 I = 1,N 
DO 100 J = 1,L 
100 C(I,J) = O. 
DO 110 I = 1,N 
ПОСТТО тти 
DO 110 K = 1,M 
110 C(I,J) =C(I,J) + A(I,K)*B(K,J) 
RETURN 
END 


ее ее ее еее еее еек 


SUBROUTINE WHICH COMPUTES THE TRANSPOSE OF THE INPUT MATRIX 
хит их иу 


SUBROUTINE TRANS(A,N,M,C,ND,MD) 
DIMENSION A(ND,MD),C(MD,ND) 
DO 100 I = 1,N 


DO 100 J = 1,M 
100 C(J,I) - А(І,7) 

RETURN 

END 


у их 
SUBROUTINE WHICH CALCULATES THE RECIPROCAL OF THE INPUT MATRIX 


еее еее ке і 


SUBROUTINE RECIP(A,C,N) 
DIMENSION A(N,N),C(N,N),D(20, 20) 
DO 100 I = 1,N 
DO 100 J = 1,N 
100 ШИР т) 
О т 
DO 115 J = N+1,2*N 
115 D(I,J) = 0.0 
DO 120 I = 1,N 
J=I+N 
120 DCI,J) = 1.0 
DO 240 K= 1,N 


M SEE 
ТЕПЕК КО, N) GOTO 180 
L = K 


DO 140 I = M,N 
140 IF (ABS(D(I,K)) .GT. ABS(D(L,K))) L=I 
ТЕ (1 80 КЕ) Gono 186 
DO 160 J = K,2*N 
TEMP = D(K,J) 
D(K,J) = D(L,J) 
160 D(L,J) = TEMP 
180 DO 185 J = M,2*N 
185 D(K,J) = D(K,J)/D(K,K) 
IF (K ВО 1) Core 220 


М1 =К- 1 
ро 200 І = 1,М1 
DO 200 J = M,2*N 
200 D(I,J) = D(I,J) - D(I,K)*D(K,J) 


92 


и. ВО. N) GOTO 260 
220 DO 240 I =M,N 
DO 240 J = M,2*N 
240 D(I,J) = D(I,J) - DCI,K)*D(K,J) 
260 DO 265 I = 1,N 
DO 265 J = 1,N 
=J +N 
265 GCISI) 2 D(I,K) 
RETURN 
END 


х» л л ХХ ХХ dese de 3e Je Je Je Jede Jede 


SUBROUTINE WHICH INITIALIZES SEVERAL MATRICES 
Хилу хе 


p pl qp lep sp) 


SUBROUTINE INITS 
COMMON W(6,6),XKK(6),PHI(6,6),Q(6,6),XKKM1(6), 
* A(6,6), B(6,6), H(6,6), НІ(6,6), PKKM1(6,6) ,PKK(6,6) 
DO 190 I =1, 
XKK( 1) 
DO 190 J 
0160 849 
PHI(I,J 
АСТ, Л) 
B(I,J) 
H(I,J) 
НІ(І, ы 


~ 
о 


СКЕ 
е е e 2 ° ON 
© 


H Wn te te ul 


20 


РККМ1(1, От 
) 1000000.0 


ДАМО] 
190 CONTINUE 

A(1,2) 
А(3,4) 
20520) 
H 11) 
НЗ) 
HC) 
RETURN 
END 


l H H H Il 
кшк ҥш кыю ы 


С 
С хїї ул жу Уу уул жу УУ УУ ese ge ge ge ge ge ge ge Fe ve Fe ke Fe Fe Fe 
C SUBROUTINE WHICH ALLOWS CHANGING OF W AND PKKM1 TO ALTER THE 
С | BEHAVIOR OF THE FILTER WITHOUT HAVING TO RE-COMPILE THE PROGRAM 
CO Arrr kkk kA kkk ekek nekk kkk kke kR kkk k kkk kkk kkk kkk kkk kkk kki Vedesede Jevedee 
C 
SUBROUTINE CHANGE(W,PKKM1) 
CHARACTER*1 ASK 
DIMENSION W(6,6),PKKM1(6,6) 
REAL*4 X 
5 FORMAT(A1) 
WRITE(*,10)W(1,1) 
10 FORMAT(' W(1,1) IS THIS VALUE: ' ,F11. 5) 
WEEE Gs, 20 ) 
20 FORMAT(' ENTER (Y) TO CHANGE IT. ') 
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245, 


30 


35 
40 
50 


READ(*,5)ASK 
X-W( 1,1) 
IF (ASK . EQ. 'Y' .OR. ASK . EQ. 'y') THEN 
WRITE(*,40) 
READ(**,50)X 
ENDIF 
DO 25 пет о 
УСТ, Т)=Х 
CONTINUE 
WRITE(*,30)PKKM1(1,1) 
FORMAT(' PKKMI(I,I) IS THIS VALUE: ' ,F10. 4) 
WRITE(*,20) 
READ(**,5)ASK 
X-PKKM1(1,1) 
IF (ASK . EQ. 'Y' .OR. ASK . EQ. 'y') THEN 
WRITE(*,40) 
READ(**,50)X 
ENDIF 
DO 35 1=1,6 
РККМ1(1,1)=Х 
СОМТТМОЕ 


FORMAT(' ENTER A VALUE FOR ELEMENTS(I,I) WITH A DECIMAL POINT. ') 


FORMAT(G10. 5) 
RETURN 
END 
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APPENDIX B. INPUT DATA FILE FORMATTING PROGRAM 


This program is designed to set up properly formatted files for use as input data by 
the main program shown in Appendix A. It is an abbreviated version of a program 
presented in [Ref. 18] which was developed to create usable data files. 

The program first reads in the names of the desired input and output files and the 
array numbers of interest. An option is given the user to remove unwanted headers from 
the input data file, which some of the available files have, then the target of interest is 
requested. Finally, the program writes to the output file only those data points that 


meet the specified criteria or issues an error message if appropriate. 


G ее еее ее еее A I О 
С PROGRAM TO READ IN RAW DATA FROM KEYPORT HYDROPHONE ARRAYS, 
C SEGREGATE IT BY USER SPECIFIED MODE, AND RETAIN DATA ONLY 
С FROM THE HYDROPHONES SELECTED. INPUT AND OUTPUT FILE NAMES 
C ae ш Ор ВНЕ раа, 
С Ч s ЭЛ: Элл 2 ХҮ eyes ezevezesestesle de sese о eee ae oaa qo oo ese dedeZev es Peel e pe e eo Ze 
C 
CHARACTER DO*2, DSNAME*13, OUTFIL*13 
CHARACTER*3 ENDCHK 
CHARACTER*1 C 
INTEGER PC, ARRAY, NHEAD,A1,A2,NUM 
WRITE(* ,*) — ' ENTER THE NAME OF YOUR INPUT FILE: ' 
READ(*, (A). ) DSNAME 
WRITE(*,*) | "INPUT NAME OF DESIRED OUTPUT FILE’ 
READ(*, (A). DOU TE TG 
WRITE(*,*) ‘ENTER NUMBERS OF ARRAYS TO BE PAIRED ' 
КЕАП(%,%) А1,А2 
ОРЕМ(1, .FILE-DSNAME, STATUS = OLD.) 
OPEN(2,FILE=OUTFIL,STATUS='NEW') 
WRITE(*,*)' DO YOU WANT TO REMOVE HEADERS? (Y or мэ! 
READC=. (A) ) C 
IF(C.EQ. 'N') GOTO 12 
11 READ(1,120)ENDCHK 
WRITE(*,*) ENDCHK 
IF(ENDCHK. NE. 'END') GOTO 11 
12 CONTINUE 
WRITE(*,*)' INPUT MODE TO BE KEPT?' 
READ(*,*) NUM 
10 READ(1,100,END=50)PC,DO,X,Y,Z,ARRAY , MODE 


сл 
сл 


20 


50 

100 
110 
120 


IF(DO .EQ. 'EV')GOTO 20 

IFCMODE „МЕ. NUM) T COTOT2O 

IF( (ARRAY .NE. A1). . AND. (ARRAY (Е”2 201000712220 
WRLTE(2,110)PC, X, Y^ Z ARRAY, MODE 

CONTINUE 

GOTO 10 


WRITE(*,*)' THERE WAS AN ERROR IN THE FILE' 
CONTINUE | 
FORMAT( 15 A251. F 7. 152. Еү7 172295 ШО Ш, 
FORMAT( 1X 15 22 2F10.1 2X 12 25 TION) 

FORMAT(31X,A3) 

CLOSE (UNIT=1) 

CLOSE( UNIT=2) 

END 
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